#include <basic.h>
#include <tk/tkernel.h>
#include <stdio.h>
#include <math.h>
#include "gl.h"
#include "Tkgles.h"
#ifdef DEBUG
#include <util/tmonitor.h>
#endif
#include "functions.h"
#define INI_FPSCR 0x00040000




/******************************************************************
main
******************************************************************/
LOCAL	INT ang = 0;
LOCAL SYSTIM pk_time,prev_time;
void initGL(){

	glViewport(0, -320, 240, 320);

	glMatrixMode(GL_PROJECTION);
   //glLoadIdentity();
	glFrustumf(-6.0f,6.0f,-8.0f,8.0f,4.5f,15.0f);

	glMatrixMode(GL_MODELVIEW);
	
	printf("initGL finished\n");
}

void draw_scene(){
	zBufferInit();
    ClearScreenBuffer();
	GLfloat vertices[4][3];
    GLfloat color[4][4];

	//glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glEnableClientState(GL_VERTEX_ARRAY);

	glVertexPointer(3, GL_FLOAT, 0, vertices);
    glColorPointer(4, GL_FLOAT, 0, color);

	glLoadIdentity();
	glTranslatef(0.0f, 0.0f, -7.0f);
	glRotatef(PI/180*ang, 0.0f, 1.0f,0.0f);

    vertices[0][0]=3.0f;  vertices[0][1]=2.0f;  vertices[0][2]=1.0f;
	vertices[1][0]=3.0f; vertices[1][1]=-2.0f;  vertices[1][2]=1.0f;
	vertices[2][0]=-3.0f;  vertices[2][1]=2.0f; vertices[2][2]=1.0f;
    vertices[3][0]=-3.0f;  vertices[3][1]=-2.0f; vertices[3][2]=1.0f;

    color[0][0] = 1.0f; color[0][1] = 0.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 0.0f; color[1][1] = 1.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 0.0f; color[2][1] = 0.0f; color[2][2] = 1.0f; color[2][3] = 1.0f;
    color[3][0] = 1.0f; color[3][1] = 0.0f; color[3][2] = 0.0f; color[3][3] = 1.0f;

	glDrawArrays(GL_TRIANGLES, 1, 3);


/* Draw a house
    //Front face
	vertices[0][0]=3.0f;  vertices[0][1]=2.0f;  vertices[0][2]=2.0f;
	vertices[1][0]=3.0f; vertices[1][1]=-2.0f;  vertices[1][2]=2.0f;
	vertices[2][0]=-3.0f;  vertices[2][1]=2.0f; vertices[2][2]=2.0f;
    vertices[3][0]=-3.0f;  vertices[3][1]=-2.0f; vertices[3][2]=2.0f;

    color[0][0] = 1.0f; color[0][1] = 1.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 1.0f; color[1][1] = 1.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 1.0f; color[2][1] = 1.0f; color[2][2] = 0.0f; color[2][3] = 1.0f;
    color[3][0] = 0.0f; color[3][1] = 1.0f; color[3][2] = 1.0f; color[3][3] = 1.0f;

	glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);


    //right side
    vertices[0][0]=3.0f;  vertices[0][1]=2.0f;  vertices[0][2]=-2.0f;
	vertices[1][0]=3.0f; vertices[1][1]=-2.0f;  vertices[1][2]=-2.0f;
	vertices[2][0]=3.0f;  vertices[2][1]=2.0f; vertices[2][2]=2.0f;
    vertices[3][0]=3.0f;  vertices[3][1]=-2.0f; vertices[3][2]=2.0f;

    color[0][0] = 0.0f; color[0][1] = 1.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 0.0f; color[1][1] = 1.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 0.0f; color[2][1] = 1.0f; color[2][2] = 0.0f; color[2][3] = 1.0f;
    color[3][0] = 0.0f; color[3][1] = 1.0f; color[3][2] = 0.0f; color[3][3] = 1.0f;

    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

    //Back face
	vertices[0][0]=3.0f;  vertices[0][1]=2.0f;  vertices[0][2]=-2.0f;
	vertices[1][0]=3.0f; vertices[1][1]=-2.0f;  vertices[1][2]=-2.0f;
	vertices[2][0]=-3.0f;  vertices[2][1]=2.0f; vertices[2][2]=-2.0f;
    vertices[3][0]=-3.0f;  vertices[3][1]=-2.0f; vertices[3][2]=-2.0f;

    color[0][0] = 0.0f; color[0][1] = 1.0f; color[0][2] = 1.0f; color[0][3] = 1.0f;
    color[1][0] = 0.0f; color[1][1] = 1.0f; color[1][2] = 1.0f; color[1][3] = 1.0f;
    color[2][0] = 0.0f; color[2][1] = 1.0f; color[2][2] = 1.0f; color[2][3] = 1.0f;
    color[3][0] = 0.0f; color[3][1] = 1.0f; color[3][2] = 1.0f; color[3][3] = 1.0f;

    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

    //Left side
    vertices[0][0]=-3.0f;  vertices[0][1]=2.0f;  vertices[0][2]=-2.0f;
	vertices[1][0]=-3.0f; vertices[1][1]=-2.0f;  vertices[1][2]=-2.0f;
	vertices[2][0]=-3.0f;  vertices[2][1]=2.0f; vertices[2][2]=2.0f;
    vertices[3][0]=-3.0f;  vertices[3][1]=-2.0f; vertices[3][2]=2.0f;

    color[0][0] = 0.0f; color[0][1] = 1.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 0.0f; color[1][1] = 1.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 0.0f; color[2][1] = 1.0f; color[2][2] = 0.0f; color[2][3] = 1.0f;
    color[3][0] = 0.0f; color[3][1] = 1.0f; color[3][2] = 0.0f; color[3][3] = 1.0f;

    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
    //Front roof
    vertices[0][0]=3.2f;  vertices[0][1]=1.6f;  vertices[0][2]=2.5f;
	vertices[1][0]=3.2f; vertices[1][1]=4.0f;  vertices[1][2]=0.0f;
	vertices[2][0]=-3.2f;  vertices[2][1]=1.6f; vertices[2][2]=2.5f;
    vertices[3][0]=-3.2f;  vertices[3][1]=4.0f; vertices[3][2]=0.0f;

    color[0][0] = 1.0f; color[0][1] = 0.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 1.0f; color[1][1] = 0.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 1.0f; color[2][1] = 0.0f; color[2][2] = 0.0f; color[2][3] = 1.0f;
    color[3][0] = 1.0f; color[3][1] = 0.0f; color[3][2] = 0.0f; color[3][3] = 1.0f;

	glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

    //Rear roof
    vertices[0][0]=3.2f;  vertices[0][1]=1.6f;  vertices[0][2]=-2.5f;
	vertices[1][0]=3.2f; vertices[1][1]=4.0f;  vertices[1][2]=0.0f;
	vertices[2][0]=-3.2f;  vertices[2][1]=1.6f; vertices[2][2]=-2.5f;
    vertices[3][0]=-3.2f;  vertices[3][1]=4.0f; vertices[3][2]=0.0f;

    color[0][0] = 1.0f; color[0][1] = 0.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 1.0f; color[1][1] = 0.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 1.0f; color[2][1] = 0.0f; color[2][2] = 0.0f; color[2][3] = 1.0f;
    color[3][0] = 1.0f; color[3][1] = 0.0f; color[3][2] = 0.0f; color[3][3] = 1.0f;

	glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

    //Cua chinh
    vertices[0][0]=0.7f;  vertices[0][1]=0.5f;  vertices[0][2]=2.2f;
	vertices[1][0]=0.7f; vertices[1][1]=-2.0f;  vertices[1][2]=2.2f;
	vertices[2][0]=-0.7f;  vertices[2][1]=0.5f; vertices[2][2]=2.2f;
    vertices[3][0]=-0.7f;  vertices[3][1]=-2.0f; vertices[3][2]=2.2f;

    color[0][0] = 1.0f; color[0][1] = 0.0f; color[0][2] = 0.0f; color[0][3] = 1.0f;
    color[1][0] = 1.0f; color[1][1] = 0.0f; color[1][2] = 0.0f; color[1][3] = 1.0f;
    color[2][0] = 1.0f; color[2][1] = 0.0f; color[2][2] = 0.0f; color[2][3] = 1.0f;
    color[3][0] = 1.0f; color[3][1] = 0.0f; color[3][2] = 0.0f; color[3][3] = 1.0f;

	glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);

*/
	glDisableClientState(GL_VERTEX_ARRAY);

    flush_display();
	//glFinish();
}
extern float vect1[4] = {0,0,0,0};
extern float vect2[4] = {0.1f,0.2f,0.3f,0.4f};
extern float vect3[4] = {0.5f,0.6f,0.7f,0.8f};
float mat1[4][4] = {{1,2,0,1},{2,1,3,2},{1,1,1,-1},{2,-3,0,1}};
float mat2[4][4] = {{1,2,3,4},{2,3,4,5},{3,4,5,6},{4,5,6,7}};
float mat3[4][4];
LOCAL ID taskid1;
LOCAL void task1(INT stacd, VP exinf){
	INT count = 0;

	//set_fpscr(INI_FPSCR);
	ld_ext(mat1);
	mtrx4mul(mat2,mat3);
	printf("Matrix: %f, %f, %f, %f\n",mat3[0][0],mat3[1][1],mat3[2][2],mat3[3][3]);
	
	ld_ext(mat2);
	mtrx4mul(mat1,mat3);
	printf("Matrix: %f, %f, %f, %f\n",mat3[0][0],mat3[1][1],mat3[2][2],mat3[3][3]);
	printf("%f, %f, %f, %f\n", vect1[0],vect1[1],vect1[2],vect1[3]);
	add4(vect2,vect2,vect1);
	printf("%f, %f, %f, %f\n", vect1[0],vect1[1],vect1[2],vect1[3]);

    //ang = 183;
	//draw_scene();

	while (1){
		tk_get_tim(&pk_time);
		if (pk_time.lo - prev_time.lo > 40){
			prev_time = pk_time;
				if ( ang == 359) ang = 0;
				else ang += 1;
            //printf("Angle %d\n",ang);
			draw_scene();
			count++;
			//flush_display();
		}
	}

	tk_exd_tsk();
}

EXPORT ER main( INT ac, UB *av[] )
{
	ER ercd = 0;
	T_CTSK ctsk;


#ifdef DEBUG
	tm_monitor();
#endif
	if( ac < 0){
		tk_ter_tsk(taskid1);
		tk_del_tsk(taskid1);
		goto ext;
	}

//------my code begin here--------------------------------------------

	TkGlInit();
	initGL();
	//draw_scene();



	ctsk.exinf = (VP)0x74736574; ///thong tin mo rong cua task
	ctsk.tskatr = TA_HLNG | TA_RNG1 | TA_FPU; ///thiet lap thuoc tinh cho task
	ctsk.task = task1; /// thiet lap dia chi bat dau cua task
	ctsk.itskpri = 140; /// do uu tien cua task
	ctsk.stksz = 1024 * 4; ///kich thuoc can co' cua task de task thuc thi
	taskid1 = tk_cre_tsk(&ctsk); // tao task voi cac thuoc tinh da~ duoc thiet lap truoc
	tk_sta_tsk(taskid1, 0);

/*
	float vect3[4];
	float vect1[4] = {1,2,3,4};
	float vect2[4] = {2,3,4,5};
	_Sub4(vect1,vect2,vect3);
	printf("%f, %f, %f, %f\n", vect3[0],vect3[1],vect3[2],vect3[3]);
*/
//	int a = 0x01020304;
//	char * b = (char *)&a;
//	printf("%u %u %u %u\n", b[0],b[1],b[2],b[3]);

ext:
	printf("main ended with code %d\n",ercd);
	return 0;
}


